copp\copp\copp3/
interpolation.rs

1//! Interpolation and profile-conversion utilities for third-order path parameterization.
2//!
3//! # Method identity
4//! This module serves both:
5//! - **Time-Optimal Path Parameterization (TOPP3)** workflows,
6//! - **Convex-Objective Path Parameterization (COPP3)** workflows.
7//!
8//! # Scope
9//! This module provides deterministic conversions between:
10//! - node profiles `a(s) = \dot{s}^2` and `b(s) = \ddot{s}` sampled on stations,
11//! - time mapping `t(s)`,
12//! - inverse sampling `s(t)`.
13//!
14//! # Conventions
15//! - Path grid uses station samples `s[0..=n]`.
16//! - Both `a` and `b` are node-based in TOPP3/COPP3 (`a.len() == b.len() == s.len()`).
17//! - `num_stationary = (head, tail)` indicates stationary boundary counts at start/end.
18
19use crate::copp::InterpolationMode;
20use crate::copp::copp3::{Topp3ProfileMut, Topp3ProfileRef};
21use crate::diag::{
22    CoppError, check_input_len_at_least, check_input_len_equal, check_input_non_negative,
23    check_input_not_empty, check_input_not_nan_infinite, check_input_slice_non_negative,
24    check_input_slice_not_nan_infinite, check_input_strictly_increasing,
25};
26use crate::math::numerical::{EPS_ZERO, solve_2x2};
27use itertools::izip;
28
29/// Compute cumulative time profile `t(s)` from a TOPP3/COPP3 profile.
30///
31/// # Semantics
32/// - `t_s[i]` is the time at station `s[i]`.
33/// - initial condition is `t_s[0] = t0`.
34/// - returns `(t_final, t_s)` where `t_final == *t_s.last().unwrap()`.
35///
36/// # Input contract
37/// - valid when `s.len() >= 2 + profile.2.0 + profile.2.1`;
38/// - requires `profile.0.len() == s.len()` and `profile.1.len() == s.len()`;
39/// - all inputs must contain only finite values;
40/// - `s` must be strictly increasing.
41///
42/// # Returns
43/// Returns `(t_final, t_s)` where `t_s[i]` is cumulative time at `s[i]`.
44///
45/// # Errors
46/// Returns [`CoppError::InvalidInput`](crate::diag::CoppError::InvalidInput) when dimensions, stationary counts,
47/// monotonicity, positivity, or numeric finiteness requirements are violated.
48///
49/// # Contract
50/// - `t_s.len() == s.len()` on valid input.
51/// - `t_s[0] == t0` on valid input.
52pub fn s_to_t_topp3(
53    s: &[f64],
54    profile: Topp3ProfileRef<'_>,
55    t0: f64,
56) -> Result<(f64, Vec<f64>), CoppError> {
57    check_topp3_sab("s_to_t_topp3", s, profile)?;
58    check_input_not_nan_infinite("s_to_t_topp3", "t0", t0)?;
59    let (a, b, num_stationary) = profile;
60    let mut t_s = Vec::<f64>::with_capacity(s.len()); // t_s[i] = t(s[i]), begin from t0
61    let mut t_prev = t0;
62    let n = s.len() - 1;
63    t_s.push(t_prev);
64    if num_stationary.0 > 0 {
65        let s0 = s.first().unwrap();
66        t_s.resize(1 + num_stationary.0, t_prev);
67        for (t_curr, a_curr, s_curr) in izip!(t_s.iter_mut(), a.iter(), s.iter()).skip(1) {
68            *t_curr += 3.0 * (s_curr - s0) / a_curr.sqrt();
69        }
70        t_prev = *t_s.last().unwrap();
71    }
72    for (s_pair, b_pair, a_curr) in izip!(s.windows(2), b.windows(2), a.iter())
73        .skip(num_stationary.0)
74        .take(n - num_stationary.0 - num_stationary.1)
75    {
76        t_prev += integral_rsrqp(
77            *a_curr,
78            2.0 * b_pair[0],
79            (b_pair[1] - b_pair[0]) / (s_pair[1] - s_pair[0]),
80            0.0,
81            s_pair[1] - s_pair[0],
82        );
83        t_s.push(t_prev);
84    }
85    if num_stationary.1 > 0 {
86        let s_final = s.last().unwrap();
87        let t_final =
88            t_prev + 3.0 * (s_final - s[n - num_stationary.1]) / a[n - num_stationary.1].sqrt();
89        t_s.resize(s.len(), t_final);
90        if num_stationary.1 > 1 {
91            for (t_curr, a_curr, s_curr) in izip!(t_s.iter_mut(), a.iter(), s.iter())
92                .rev()
93                .skip(1)
94                .take(num_stationary.1 - 1)
95            {
96                *t_curr += 3.0 * (s_curr - s_final) / a_curr.sqrt();
97            }
98        }
99    }
100
101    let t_final = *t_s.last().unwrap();
102    if !t_final.is_finite() || t_s.iter().any(|value| !value.is_finite()) {
103        return Err(CoppError::InvalidInput(
104            "s_to_t_topp3".into(),
105            "computed time profile contains NaN or infinity".into(),
106        ));
107    }
108    check_input_strictly_increasing("s_to_t_topp3", "t_s", &t_s)?;
109    Ok((t_final, t_s))
110}
111
112/// Compute definite integral of reciprocal-square-root quadratic polynomial:
113/// $$dt = \int_{x_{left}}^{x_{right}} \frac{dx}{\sqrt{c_0 + c_1 x + c_2 x^2}}.$$
114fn integral_rsrqp(c0: f64, c1: f64, c2: f64, x_left: f64, x_right: f64) -> f64 {
115    if c2 > f64::EPSILON {
116        let func = |x: f64| x + 0.5 * c1 / c2 + (x * x + (c1 * x + c0) / c2).sqrt();
117        (func(x_right).abs().ln() - func(x_left).abs().ln()) / c2.sqrt()
118    } else if c2 < -f64::EPSILON {
119        let delta = c1 * c1 - 4.0 * c2 * c0;
120        if delta > 0.0 {
121            let func = |x: f64| (-2.0 * c2 * x - c1) / delta.sqrt();
122            (func(x_right).asin() - func(x_left).asin()) / (-c2).sqrt()
123        } else {
124            f64::INFINITY
125        }
126    } else if c1.abs() > f64::EPSILON {
127        // Dt = \int_{xl}^{xr} dx/sqrt(C1*x+C0)
128        2.0 / c1 * ((c1 * x_right + c0).sqrt() - (c1 * x_left + c0).sqrt())
129    } else if c0.abs() > f64::EPSILON {
130        // Dt = \int_{xl}^{xr} dx/sqrt(C0)
131        (x_right - x_left) / c0.sqrt()
132    } else {
133        f64::INFINITY
134    }
135}
136
137/// Interpolate inverse mapping `s(t)` from a TOPP3/COPP3 profile and sampled `t(s)`.
138///
139/// # Modes
140/// - [`UniformTimeGrid`](crate::InterpolationMode::UniformTimeGrid)`(t0, dt, include_final)`: generate uniform time samples;
141/// - `NonUniformTimeGrid(t_sample)`: use caller-provided increasing samples.
142///
143/// # Input contract
144/// - requires `s.len() >= 2`, profile slice lengths equal to `s.len()`, and `t_s.len() == s.len()`;
145/// - requires `t_s` strictly increasing;
146/// - all profile and time-grid values must be finite.
147///
148/// # Output semantics
149/// - output length matches requested sample count in each mode;
150/// - for out-of-range time samples, output entries are `NaN`.
151///
152/// # Returns
153/// Returns sampled `s(t)` values under the requested interpolation `mode`.
154///
155/// # Errors
156/// Returns [`CoppError::InvalidInput`](crate::diag::CoppError::InvalidInput) when dimensions, stationary counts,
157/// monotonicity, positivity, or numeric finiteness requirements are violated.
158///
159/// # Contract
160/// - preserves caller time-sample ordering.
161/// - malformed input is reported as [`CoppError::InvalidInput`](crate::diag::CoppError::InvalidInput).
162pub fn t_to_s_topp3(
163    s: &[f64],
164    profile: Topp3ProfileRef<'_>,
165    t_s: &[f64],
166    mode: InterpolationMode<'_>,
167) -> Result<Vec<f64>, CoppError> {
168    check_topp3_sab("t_to_s_topp3", s, profile)?;
169    check_input_len_equal(
170        "t_to_s_topp3",
171        "`t_s.len()`",
172        t_s.len(),
173        "`s.len()`",
174        s.len(),
175    )?;
176    check_input_slice_not_nan_infinite("t_to_s_topp3", "t_s", t_s)?;
177    check_input_strictly_increasing("t_to_s_topp3", "t_s", t_s)?;
178    match mode {
179        InterpolationMode::UniformTimeGrid(t0, dt, include_final) => {
180            check_input_not_nan_infinite("t_to_s_topp3", "t0", t0)?;
181            check_input_not_nan_infinite("t_to_s_topp3", "dt", dt)?;
182            if dt <= 0.0 {
183                return Err(CoppError::InvalidInput(
184                    "t_to_s_topp3".into(),
185                    format!("`dt` = {dt} must be positive"),
186                ));
187            }
188            // num_t * dt + t0 <= t_final
189            let num_t = ((t_s.last().unwrap() - t0) / dt).floor() as usize;
190            let mut s_t = t_to_s_topp3_core(
191                s,
192                profile,
193                t_s,
194                (0..num_t).map(|i| t0 + i as f64 * dt),
195                num_t,
196            );
197            if include_final {
198                let flag = if s_t.is_empty() {
199                    t0 <= *t_s.last().unwrap()
200                } else {
201                    *s_t.last().unwrap() < *s.last().unwrap()
202                };
203                if flag {
204                    s_t.push(*s.last().unwrap());
205                }
206            }
207            Ok(s_t)
208        }
209        InterpolationMode::NonUniformTimeGrid(t_sample) => {
210            check_input_not_empty("t_to_s_topp3", "`t_sample`", t_sample.len())?;
211            check_input_slice_not_nan_infinite("t_to_s_topp3", "t_sample", t_sample)?;
212            check_input_strictly_increasing("t_to_s_topp3", "t_sample", t_sample)?;
213            Ok(t_to_s_topp3_core(
214                s,
215                profile,
216                t_s,
217                t_sample.iter().cloned(),
218                t_sample.len(),
219            ))
220        }
221    }
222}
223
224/// Core inverse interpolation kernel for [`t_to_s_topp3`](crate::solver::topp3_lp::t_to_s_topp3).
225///
226/// The public wrapper validates dimensions, finiteness, station ordering, and
227/// sample ordering before calling this routine. This core then walks the time
228/// samples once and maps each sample into the corresponding station interval.
229fn t_to_s_topp3_core(
230    s: &[f64],
231    profile: Topp3ProfileRef<'_>,
232    t_s: &[f64],
233    mut t_sample: impl Iterator<Item = f64>,
234    len_t_sample: usize,
235) -> Vec<f64> {
236    let (a, b, num_stationary) = profile;
237    // Map t to s
238    let &t_start = t_s.first().unwrap();
239    let &t_final = t_s.last().unwrap();
240    let mut s_t = Vec::<f64>::with_capacity(len_t_sample + 1); // s_t[i] = s(t[i])
241    let Some(mut t_curr) = t_sample.next() else {
242        return vec![];
243    };
244    while t_curr < t_start {
245        s_t.push(f64::NAN);
246        let Some(t) = t_sample.next() else {
247            return s_t;
248        };
249        t_curr = t;
250    }
251
252    if num_stationary.0 > 0 {
253        let s0 = s.first().unwrap();
254        let a_stationary = a[num_stationary.0];
255        let t_stationary = t_s[num_stationary.0];
256        let d3u_over_6 =
257            a_stationary.sqrt() * a_stationary / (27.0 * (s[num_stationary.0] - s0).powi(2));
258        while t_curr <= t_stationary {
259            s_t.push(s0 + d3u_over_6 * (t_curr - t_start).powi(3));
260            let Some(t) = t_sample.next() else {
261                return s_t;
262            };
263            t_curr = t;
264        }
265    }
266
267    for (s_pair, &a_curr, b_pair, t_pair) in
268        izip!(s.windows(2), a.iter(), b.windows(2), t_s.windows(2))
269            .skip(num_stationary.0)
270            .take(s.len() - num_stationary.0 - num_stationary.1 - 1)
271    {
272        while t_curr <= t_pair[1] {
273            s_t.push(
274                s_pair[0]
275                    + inverse_rsrqp(
276                        a_curr,
277                        2.0 * b_pair[0],
278                        (b_pair[1] - b_pair[0]) / (s_pair[1] - s_pair[0]),
279                        0.0,
280                        t_curr - t_pair[0],
281                    ),
282            );
283            let Some(t) = t_sample.next() else {
284                return s_t;
285            };
286            t_curr = t;
287        }
288    }
289
290    if num_stationary.1 > 0 {
291        let s_final = s.last().unwrap();
292        let a_stationary = a[s.len() - num_stationary.1 - 1];
293        let d3u_over_6 = a_stationary.sqrt() * a_stationary
294            / (27.0 * (s_final - s[s.len() - num_stationary.1 - 1]).powi(2));
295        while t_curr <= t_final {
296            s_t.push(s_final + d3u_over_6 * (t_curr - t_final).powi(3));
297            let Some(t) = t_sample.next() else {
298                return s_t;
299            };
300            t_curr = t;
301        }
302    }
303
304    s_t.push(f64::NAN);
305    while t_sample.next().is_some() {
306        s_t.push(f64::NAN);
307    }
308    s_t
309}
310
311/// Solve `x_right` from
312/// $$dt = \int_{x_{left}}^{x_{right}} \frac{dx}{\sqrt{c_0 + c_1 x + c_2 x^2}}.$$
313fn inverse_rsrqp(c0: f64, c1: f64, c2: f64, x_left: f64, dt: f64) -> f64 {
314    if dt == 0.0 {
315        return x_left;
316    }
317    let delta = c1 * c1 - 4.0 * c2 * c0;
318    if c2 > f64::EPSILON {
319        let mu = (c2.sqrt() * dt
320            + (x_left + 0.5 * c1 / c2 + (x_left * x_left + (c1 * x_left + c0) / c2).sqrt())
321                .abs()
322                .ln())
323        .exp();
324        let xr1 = -0.5 * c1 / c2 + 0.5 * (mu + delta / (4.0 * c2 * c2 * mu));
325        let xr2 = -0.5 * c1 / c2 - 0.5 * (mu + delta / (4.0 * c2 * c2 * mu));
326        let mut flag1 = true;
327        let mut flag2 = true;
328        if dt > 0.0 {
329            flag1 &= xr1 > x_left;
330            flag2 &= xr2 > x_left;
331        } else {
332            flag1 &= xr1 < x_left;
333            flag2 &= xr2 < x_left;
334        }
335        if flag1 && flag2 {
336            let dt1 = integral_rsrqp(c0, c1, c2, x_left, xr1);
337            let dt2 = integral_rsrqp(c0, c1, c2, x_left, xr2);
338            if (dt1 - dt).abs() < (dt2 - dt).abs() {
339                xr1
340            } else {
341                xr2
342            }
343        } else if flag1 {
344            xr1
345        } else if flag2 {
346            xr2
347        } else {
348            f64::INFINITY
349        }
350    } else if c2 < -f64::EPSILON {
351        (c1 + delta.sqrt()
352            * ((-c2).sqrt() * dt + ((-2.0 * c2 * x_left - c1) / delta.sqrt()).asin()).sin())
353            / (-2.0 * c2)
354    } else if c1.abs() > f64::EPSILON {
355        ((0.5 * c1 * dt + (c1 * x_left + c0).sqrt()).powi(2) - c0) / c1
356    } else if c0.abs() > f64::EPSILON {
357        c0.sqrt() * dt + x_left
358    } else {
359        f64::INFINITY
360    }
361}
362
363/// Post-process a mutable `(a, b)` profile so that interpolated `a(s)` stays strictly positive per interval.
364///
365/// This is a numerical safety utility for downstream timing integration on
366/// profiles that may be very close to zero due to finite precision.
367///
368/// # Returns
369/// Returns `true` when in-place adjustment succeeds, otherwise `false`.
370///
371/// # Errors
372/// Returns [`CoppError::InvalidInput`](crate::diag::CoppError::InvalidInput) when dimensions, station ordering, profile
373/// positivity, stationary counts, or numeric finiteness requirements are violated.
374///
375/// # Contract
376/// - requires `a.len() == b.len() == s.len()` and `s.len() >= 4`;
377/// - requires endpoint `a` values to be nonnegative.
378pub fn force_positive_a(
379    profile: Topp3ProfileMut<'_>,
380    s: &[f64],
381    a_min: f64,
382) -> Result<bool, CoppError> {
383    let (a, b, num_stationary) = profile;
384    let n = s.len();
385    if a.len() != n || b.len() != n {
386        return Err(CoppError::InvalidInput(
387            "force_positive_a".into(),
388            format!(
389                "`a.len()` = {} and `b.len()` = {} must equal `s.len()` = {}",
390                a.len(),
391                b.len(),
392                n
393            ),
394        ));
395    }
396    if n < 4 {
397        return Err(CoppError::InvalidInput(
398            "force_positive_a".into(),
399            format!("`s.len()` = {n} must be at least 4"),
400        ));
401    }
402    check_stationary_counts("force_positive_a", n, num_stationary)?;
403    check_input_slice_not_nan_infinite("force_positive_a", "s", s)?;
404    check_input_slice_non_negative("force_positive_a", "a", a)?;
405    check_input_slice_not_nan_infinite("force_positive_a", "b", b)?;
406    check_input_non_negative("force_positive_a", "a_min", a_min)?;
407    check_input_strictly_increasing("force_positive_a", "s", s)?;
408    // Now we have a(s[i]) >= 0, and we would like to modify a(s) > 0 for s in (s[i], s[i+1]) if a(s) can be negative for some s in (s[i], s[i+1]).
409    let mut flag_succeed = true;
410    for i in (num_stationary.0 + 1)..(n - 2 - num_stationary.1) {
411        // Consider a[i-1], a[i], a[i+1], a[i+2]
412        let b1 = b[i];
413        let b2 = b[i + 1];
414        if b1 < 0.0 && b2 > 0.0 {
415            // a(s) = a[i] + 2 * b[i] * (s - s[i]) + (b[i+1] - b[i]) / ds1 * (s - s[i])^2
416            // b[i] ^ 2 < a[i] * (b[i+1] - b[i]) / ds1 should hold
417            // b[i] ^ 2 * ds1 < a[i] * (b[i+1] - b[i]) should hold
418            let s1 = s[i];
419            let s2 = s[i + 1];
420            let ds1 = s2 - s1;
421            let a1 = a[i];
422            let amin = a_min.max(a1.min(a[i + 1]));
423            let amin = if amin > 10.0 * EPS_ZERO {
424                0.1 * amin
425            } else if amin > EPS_ZERO {
426                EPS_ZERO
427            } else {
428                amin
429            };
430            let da = a1 - amin;
431            let db = b2 - b1;
432            if b1 * b1 * ds1 >= da * db {
433                // a(s) <= 0 holds in (s[i], s[i+1])
434                // We add c0 on (s[i-1],s[i+2]), c1 on (s[i],s[i+2]), and c2 on (s[i+1],s[i+2])
435                // x[i-1] and x[i+2] should keep the same.
436                // (i) --- c0*(s[i+2] - s[i-1]) + c1*(s[i+2] - s[i]) + c2*(s[i+2] - s[i+1]) == 0
437                // (ii) --- c0*(s[i+2] - s[i-1])^2 + c1*(s[i+2] - s[i])^2 + c2*(s[i+2] - s[i+1])^2 == 0
438                let s0 = s[i - 1];
439                let s3 = s[i + 2];
440                let delta_s_end = (s3 - s0, s3 - s1, s3 - s2);
441                let coeff = match solve_2x2(
442                    (
443                        (delta_s_end.1, delta_s_end.2),
444                        (delta_s_end.1 * delta_s_end.1, delta_s_end.2 * delta_s_end.2),
445                    ),
446                    (-delta_s_end.0, -delta_s_end.0 * delta_s_end.0),
447                ) {
448                    Some(coeff) => {
449                        // A*[c1;c2] = b*c0
450                        coeff
451                    }
452                    None => {
453                        crate::verbosity_log!(
454                            crate::diag::Verbosity::Debug,
455                            "coeff is None? A = {:?}, b = {:?}",
456                            (
457                                (delta_s_end.1, delta_s_end.2),
458                                (delta_s_end.1 * delta_s_end.1, delta_s_end.2 * delta_s_end.2)
459                            ),
460                            (-delta_s_end.0, -delta_s_end.0 * delta_s_end.0)
461                        );
462                        flag_succeed = false;
463                        continue;
464                    }
465                };
466                // c1 = coeff.0 * c0, c2 = coeff.1 * c0
467                // Changes: a[i] += c0 * (s1-s0)^2, b[i] += c0 * (s1-s0), b[i+1] += c0 * (s2-s0) + c1 * (s2-s1)
468                let ds0 = s1 - s0;
469                let coeff_c = (ds0 * ds0, ds0, ds0 + ds1 * (1.0 + coeff.0));
470                // Changes: a[i] += c0 * coeff_c.0, b[i] += c0 * coeff_c.1, b[i+1] += c0 * coeff_c.2
471                // We hope that a(s) = a[i] + 2 * b[i] * (s - s[i]) + (b[i+1] - b[i]) / ds1 * (s - s[i])^2 >= amin holds in (s[i],s[i+1])
472                // b[i] ^ 2 * ds1 == (a[i] - amin) * (b[i+1] - b[i]) should hold for new ones.
473                // For old ones: (b[i] + coeff_c.1 * c0) ^ 2 * ds1 == (a[i] - amin + coeff_c.0 * c0) * (b[i+1] - b[i] + (coeff_c.2-coeff_c.1) * c0). Now solve c0.
474                // (coeff_c.1^2 * c0^2 + 2 * b1 * coeff_c.1 * c0 + b1 ^ 2) * ds1 == coeff_c.0 * (coeff_c.2-coeff_c.1) * c0^2 + (da * (coeff_c.2-coeff_c.1) + coeff_c.0 * db) * c0 + da * db
475                // (coeff_c.1^2 * ds1 - coeff_c.0 * (coeff_c.2-coeff_c.1)) * c0^2 + (2 * b1 * coeff_c.1 * ds1 - da * (coeff_c.2-coeff_c.1) - coeff_c.0 * db) * c0 + (b1 * b1 * ds1 - da * db) == 0
476                let coeff_solve = (
477                    coeff_c.1 * coeff_c.1 * ds1 - coeff_c.0 * (coeff_c.2 - coeff_c.1),
478                    2.0 * b1 * coeff_c.1 * ds1 - da * (coeff_c.2 - coeff_c.1) - coeff_c.0 * db,
479                    b1 * b1 * ds1 - da * db,
480                );
481                let norm = coeff_solve.0.abs() + coeff_solve.1.abs() + coeff_solve.2.abs();
482                if norm < EPS_ZERO {
483                    crate::verbosity_log!(
484                        crate::diag::Verbosity::Debug,
485                        "norm = {norm} < EPS_ZERO, coeff_solve = {coeff_solve:.8?}"
486                    );
487                    flag_succeed = false;
488                    continue;
489                }
490                let norm_inv = 1.0 / norm;
491                let coeff_solve = (
492                    coeff_solve.0 * norm_inv,
493                    coeff_solve.1 * norm_inv,
494                    coeff_solve.2 * norm_inv,
495                );
496                // coeff_solve.0 * c0^2 + coeff_solve.1 * c0 + coeff_solve.2 == 0
497                let c0 = if coeff_solve.0.abs() > EPS_ZERO {
498                    // Use quadratic formula to solve for c0
499                    let discriminant =
500                        coeff_solve.1 * coeff_solve.1 - 4.0 * coeff_solve.0 * coeff_solve.2;
501                    if discriminant < 0.0 {
502                        if coeff_c.1.abs() > EPS_ZERO && coeff_c.2.abs() > EPS_ZERO {
503                            (-b1 / coeff_c.1).min(b2 / coeff_c.2)
504                        } else if coeff_c.1.abs() > EPS_ZERO {
505                            -b1 / coeff_c.1
506                        } else if coeff_c.2.abs() > EPS_ZERO {
507                            b2 / coeff_c.2
508                        } else {
509                            crate::verbosity_log!(
510                                crate::diag::Verbosity::Debug,
511                                "discriminant = {discriminant:.8} < 0 for c0 (i={i}): coeff_solve = {coeff_solve:.8?}, coeff_c = {coeff_c:.8?}"
512                            );
513                            flag_succeed = false;
514                            continue;
515                        }
516                    } else {
517                        let sqrt_discriminant = discriminant.sqrt();
518                        // c0: (max, min)
519                        let c0 = if coeff_solve.0 > 0.0 {
520                            (
521                                (-coeff_solve.1 + sqrt_discriminant) / (2.0 * coeff_solve.0),
522                                (-coeff_solve.1 - sqrt_discriminant) / (2.0 * coeff_solve.0),
523                            )
524                        } else {
525                            (
526                                (-coeff_solve.1 - sqrt_discriminant) / (2.0 * coeff_solve.0),
527                                (-coeff_solve.1 + sqrt_discriminant) / (2.0 * coeff_solve.0),
528                            )
529                        };
530                        if c0.1 >= 0.0 { c0.1 } else { c0.0 }
531                    }
532                } else {
533                    // Linear case
534                    -coeff_solve.2 / coeff_solve.1
535                };
536                a[i] += coeff_c.0 * c0;
537                b[i] += coeff_c.1 * c0;
538                b[i + 1] += coeff_c.2 * c0;
539                a[i + 1] += (coeff_c.0 + (coeff_c.1 + coeff_c.2) * ds1) * c0;
540            }
541        }
542    }
543
544    Ok(flag_succeed)
545}
546
547/// Check the shared TOPP3 profile shape, station counts, and station ordering.
548///
549/// TOPP3/COPP3 interpolation uses node-based `a(s)` and `b(s)` profiles on the
550/// same grid, with optional stationary head/tail sections. This helper keeps
551/// those preconditions together before any timing integration is attempted.
552fn check_topp3_sab(
553    function_name: &str,
554    s: &[f64],
555    profile: Topp3ProfileRef<'_>,
556) -> Result<(), CoppError> {
557    let (a, b, num_stationary) = profile;
558    check_stationary_counts(function_name, s.len(), num_stationary)?;
559    if a.len() != s.len() || b.len() != s.len() {
560        return Err(CoppError::InvalidInput(
561            function_name.into(),
562            format!(
563                "`a.len()` = {} and `b.len()` = {} must equal `s.len()` = {}",
564                a.len(),
565                b.len(),
566                s.len()
567            ),
568        ));
569    }
570    check_input_slice_not_nan_infinite(function_name, "s", s)?;
571    check_input_slice_non_negative(function_name, "a", a)?;
572    check_input_slice_not_nan_infinite(function_name, "b", b)?;
573    check_input_strictly_increasing(function_name, "s", s)
574}
575
576/// Check that stationary head/tail counts leave at least one motion interval.
577///
578/// The minimum station count is `2 + num_stationary.0 + num_stationary.1`;
579/// checked arithmetic is used so pathological `usize` inputs are rejected as
580/// invalid input instead of overflowing.
581fn check_stationary_counts(
582    function_name: &str,
583    s_len: usize,
584    num_stationary: (usize, usize),
585) -> Result<(), CoppError> {
586    let Some(min_len) = num_stationary
587        .0
588        .checked_add(num_stationary.1)
589        .and_then(|sum| sum.checked_add(2))
590    else {
591        return Err(CoppError::InvalidInput(
592            function_name.into(),
593            "`num_stationary` overflowed while checking dimensions".into(),
594        ));
595    };
596    check_input_len_at_least(function_name, "`s.len()`", s_len, min_len)
597}
copp\copp\copp3/interpolation.rs

copp\copp\copp3/
interpolation.rs
